#include "CvFaceDetector.h"
#include "mcEdgeOp.h"

#define CV_ADJUST_FEATURES 1

static const char* numSuffix( int i )
{
    switch( i % 10 )
    {
    case 1: return "st";
    case 2: return "nd";
    case 3: return "rd";
    default: return "th";
    }
}

#ifndef _MAX_PATH
#define _MAX_PATH 1024
#endif

#define ORIG_WIN_SIZE  24
#define MIN_NEIGHBORS 2



////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
	
CvHidHaarClassifier::CvHidHaarClassifier()
{
	count		= 0;
	p_threshold	= NULL;
	p_left		= NULL;
	p_right		= NULL;
	p_alpha		= NULL;

	p_orig_feature	= NULL;
	p_feature		= NULL;
}

CvHidHaarClassifier::~CvHidHaarClassifier()
{
	Free();
}

void CvHidHaarClassifier::Free(void)
{
	count = 0;

	if (p_threshold != NULL)
		delete []p_threshold;
	p_threshold = NULL;

	if (p_left != NULL)
		delete []p_left;
	p_left = NULL;

	if (p_right != NULL)
		delete []p_right;
	p_right = NULL;

	if (p_alpha != NULL)
		delete []p_alpha;
	p_alpha = NULL;

	if (p_orig_feature != NULL)
		delete []p_orig_feature;
	p_orig_feature = NULL;

	if (p_feature != NULL)
		delete []p_feature;
	p_feature = NULL;
}

void CvHidHaarClassifier::SetNumFeatures(const int num)
{
	Free();

	if (num <= 0)
		return;
	
	count = num;

	p_threshold	= new float[count];
	p_left		= new int[count];
	p_right		= new int[count];
	p_alpha		= new float[count+1];

	p_orig_feature = new CvHaarFeature[count];
	p_feature		= new CvHidHaarFeature[count];
}

////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////

// construction & destruction
CvHidHaarStageClassifier::CvHidHaarStageClassifier()
{
	count = 0;
	p_classifier = NULL;
}

CvHidHaarStageClassifier::~CvHidHaarStageClassifier()
{
	Free();
}

void CvHidHaarStageClassifier::Free(void)
{
	count = 0;
	if (p_classifier != NULL)
		delete []p_classifier;
	p_classifier = NULL;
}

void CvHidHaarStageClassifier::SetNumClassifiers(const int num)
{
	Free();

	if (num <= 0)
		return;

	count = num;
	p_classifier = new CvHidHaarClassifier[count];
}

//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

// construction & destruction
CvHidHaarClassifierCascade::CvHidHaarClassifierCascade()
{

    pq0 = NULL;		pq1 = NULL;
	pq2 = NULL;		pq3 = NULL;

    p0 = NULL;		p1 = NULL;
	p2 = NULL;		p3 = NULL;

    p_stage_classifier = NULL;
}

CvHidHaarClassifierCascade::~CvHidHaarClassifierCascade()
{
	Free();
}

void CvHidHaarClassifierCascade::Free(void)
{
	count = 0;

	if (p_stage_classifier != NULL)
		delete []p_stage_classifier;
	p_stage_classifier = NULL;
}

void CvHidHaarClassifierCascade::SetNumStageCalssifiers(const int num)
{
	Free();

	if (num <= 0)
		return;

	count = num;
	p_stage_classifier = new CvHidHaarStageClassifier[count];
}



irmm_Size CvHidHaarClassifierCascade::GetHaarClassifierCascadeWindowSize(void)
{
    return realWindowSize;
}

double CvHidHaarClassifierCascade::GetHaarClassifierCascadeWindowScale(void)
{
    return scale;
}

static int is_equal( const void* _r1, const void* _r2, void* )
{
	const CvRectangle* r1 = (const CvRectangle*)_r1;
	const CvRectangle* r2 = (const CvRectangle*)_r2;
    int distance = (int)round(r1->width*0.2);

    return r2->x <= r1->x + distance &&
           r2->x >= r1->x - distance &&
           r2->y <= r1->y + distance &&
           r2->y >= r1->y - distance &&
           r2->width <= round( r1->width * 1.2 ) &&
           round( r2->width * 1.2 ) >= r1->width;
}


// operations
bool CvHidHaarClassifierCascade::LoadFromFile(const char *str_directory)
{
	const char** input_cascade = 0; 

	int n;
	char name[_MAX_PATH];

	int i, size = 0;
	char* ptr;
    
	origWindowSize = irmm_Size(ORIG_WIN_SIZE, ORIG_WIN_SIZE );

	for( n = 0; ; n++ )
    {
		sprintf( name, "%s/%d/AdaBoostCARTHaarClassifier.txt", str_directory, n );
        FILE* f = fopen( name, "rb" );
        if( !f )
			break;
		fseek( f, 0, SEEK_END );
		size += ftell( f ) + 1;
		fclose(f);
	}

	size += (n+1)*sizeof(char*);
	input_cascade = (const char**)malloc( size );
	ptr = (char*)(input_cascade + n + 1);

	for( i = 0; i < n; i++ )
	{
		sprintf( name, "%s/%d/AdaBoostCARTHaarClassifier.txt", str_directory, i );
		FILE* f = fopen( name, "rb" );
		fseek( f, 0, SEEK_END );
		size = ftell( f );
		fseek( f, 0, SEEK_SET );
		fread( ptr, 1, size, f );
		fclose(f);
		input_cascade[i] = ptr;
		ptr += size;
		*ptr++ = '\0';
	}
	input_cascade[n] = 0;

	if( n == 0 )
		return false;

	bool bsuccess=LoadCascadeCART(input_cascade, n, origWindowSize );

	scale = 1.0;

	return bsuccess;
}

void CvHidHaarClassifierCascade::UpdateRealWindowSize(double scale1)
{
    scale = scale1;
    realWindowSize =  irmm_Size(IntClose(round(origWindowSize.width * scale)),
				  IntClose(round(origWindowSize.height * scale)));
}

bool CvHidHaarClassifierCascade::LoadCascadeCART(const char** input_cascade, int n, irmm_Size origWindowSize)
{
	int		i,j,count1,l,dl;
	float	threshold;
    const char	*stage;

    origWindowSize = origWindowSize;
	SetNumStageCalssifiers(n);

    for (i=0; i<n; i++)
    {
		threshold = 0;
		dl = 0;
		
		stage = input_cascade[i];

        sscanf( stage, "%d%n", &count1, &dl );
        stage += dl;
        
        assert(count1 > 0);
		p_stage_classifier[i].SetNumClassifiers(count1);
		p_stage_classifier[i].two_rects = 1;

        for( j = 0; j < count1; j++ )
        {
            CvHidHaarClassifier* classifier = &(p_stage_classifier[i].p_classifier[j]);
            int k, rects = 0;
            char str[100];
            
            sscanf( stage, "%d%n", &classifier->count, &dl );
            stage += dl;

            classifier->SetNumFeatures(classifier->count);
            
            for( l = 0; l < classifier->count; l++ )
            {
                sscanf( stage, "%d%n", &rects, &dl );
                stage += dl;

                assert( rects >= 2 && rects <= CV_HAAR_FEATURE_MAX );

                for( k = 0; k < rects; k++ )
                {
					CvRectangle r;
                    int band = 0;
                    sscanf( stage, "%d%d%d%d%d%f%n",
                            &r.x, &r.y, &r.width, &r.height, &band,
                            &(classifier->p_orig_feature[l].rect[k].weight), &dl );
                    stage += dl;
                    classifier->p_orig_feature[l].rect[k].r = r;
                }
                sscanf( stage, "%s%n", str, &dl );
                stage += dl;
            
                classifier->p_orig_feature[l].tilted = strncmp( str, "tilted", 6 ) == 0;
            
                for( k = rects; k < CV_HAAR_FEATURE_MAX; k++ )
                {
                    memset( classifier->p_orig_feature[l].rect + k, 0,
                            sizeof(classifier->p_orig_feature[l].rect[k]) );
                }
                
                sscanf( stage, "%f%d%d%n", &(classifier->p_threshold[l]), 
                                       &(classifier->p_left[l]),
                                       &(classifier->p_right[l]), &dl);
                stage += dl;

                if( classifier->p_orig_feature[l].rect[2].weight == 0 ||
                    classifier->p_orig_feature[l].rect[2].r.width == 0 ||
                    classifier->p_orig_feature[l].rect[2].r.height == 0 )
                {
                    memset( &(classifier->p_orig_feature[l].rect[2]), 0,
                            sizeof(classifier->p_orig_feature[l].rect[2]) );
                }
                else
					p_stage_classifier[i].two_rects = 0;

            }
            for( l = 0; l <= classifier->count; l++ )
            {
                sscanf( stage, "%f%n", &(classifier->p_alpha[l]), &dl );
                stage += dl;
            }
        }
       
        sscanf( stage, "%f%n", &threshold, &dl );
        stage += dl;

        p_stage_classifier[i].threshold = threshold;
    }
	return true;
}
